#ifndef __bits_lock_h__
#define __bits_lock_h__

#include <errno.h>
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

#include "list.h"

#define BL_DBUG(...)
typedef struct {
        struct list_head list_entry;
        int pos;
        task_t task;
}bits_lock_entry_t;

typedef struct {
        uint64_t size;
        uint8_t *array;
        struct list_head lock_list;
        sy_spinlock_t lock;
} bits_lock_t;

static inline uint64_t bits_lock_size(uint64_t max_pos) { return max_pos / 8; }

static inline int bits_lock_create(bits_lock_t **_bl, uint64_t size)
{
        int ret;
        bits_lock_t *bl;

        ret = ymalloc((void **)&bl, sizeof(*bl));
        if (unlikely(ret))
                GOTO(err_ret, ret);

        memset(bl, 0x0, sizeof(*bl));
        
        bl->size = size;
        bl->array = (uint8_t *)malloc(size);
        if(!bl->array) {
                ret = ENOMEM;
                GOTO(err_free, ret);
        }

        memset(bl->array, 0, size);
        INIT_LIST_HEAD(&bl->lock_list);
        sy_spin_init(&bl->lock);

        *_bl = bl;
        
        return 0;
err_free:
        yfree1((void **)&bl);
err_ret:
        return ret;
}

static inline void bits_lock_destory(bits_lock_t **_bl)
{
        bits_lock_t *bl = *_bl;
        free(bl->array);
        sy_spin_destroy(&bl->lock);

        yfree((void **)&bl);
        *_bl = NULL;
        
}

static inline void bits_lock_zero(bits_lock_t *bl)
{
        memset(bl->array, 0, bl->size);
}

static inline int bits_lock_init(bits_lock_t *bl, uint64_t size, void *array)
{
        bl->size = size;
        bl->array = (uint8_t *)array;

        return 0;
}

static inline int bits_lock_is_set(bits_lock_t *bl, uint64_t pos)
{
        int l_index = pos / 8;
        int r_index = pos % 8;

        return bl->array[l_index] & (1 << r_index);
}

static inline void bits_lock_set(bits_lock_t *bl, uint64_t pos, uint32_t count)
{
        for (int i = 0; i < count; i++) {
                int l_index = pos / 8;
                int r_index = pos % 8;

                bl->array[l_index] |= 1 << r_index;

                pos++;
        }
}

static inline void bits_lock_clear(bits_lock_t *bl, uint64_t pos, uint32_t count)
{
        for (int i = 0; i < count; i++) {
                int l_index = pos / 8;
                int r_index = pos % 8;

                bl->array[l_index] &= ~(1 << r_index);

                pos++;
        }
}

static inline int bits_lock_wlock(bits_lock_t *bl, uint64_t pos)
{
        assert(pos < bl->size * 8);
        sy_spin_lock(&bl->lock);

        BL_DBUG("lock: *%p, pos:%d\r\n", bl, pos);
        if (bits_lock_is_set(bl, pos)) {
                bits_lock_entry_t *entry = malloc(sizeof(bits_lock_entry_t));
                entry->pos = pos;
                entry->task = schedule_task_get();
                list_add_tail(&entry->list_entry, &bl->lock_list);

                BL_DBUG("lock insert: %p\r\n", entry->task);
                
                sy_spin_unlock(&bl->lock);
                /*todo. is it a problem here??*/
                schedule_yield("bits_lock", NULL, NULL);
                assert(bits_lock_is_set(bl, pos));
                return 0;
        } else {
                bits_lock_set(bl, pos, 1);

                BL_DBUG("lock set: *%p\r\n", bl);
                sy_spin_unlock(&bl->lock);
                return 0;
        }
}

static inline int bits_lock_unlock(bits_lock_t *bl, uint64_t pos)
{
        struct list_head *i;
        struct list_head *n;

        assert(pos < bl->size * 8);
        sy_spin_lock(&bl->lock);

        assert(bits_lock_is_set(bl, pos));

        BL_DBUG("unlock: *%p, pos:%d\r\n", bl, pos);

        list_for_each_safe(i, n, &bl->lock_list) {
                bits_lock_entry_t *entry = (bits_lock_entry_t *)i;
                
                if(entry->pos == pos) {
                        list_del_init(&entry->list_entry);
                        schedule_resume(&entry->task, 0, NULL);

                        free(entry);

                        BL_DBUG("unlock: *%p, resume\r\n", bl);
                        sy_spin_unlock(&bl->lock);
                        return 0;
                }
        }

        BL_DBUG("unlock: *%p, clear\r\n", bl);
        bits_lock_clear(bl, pos, 1);
        
        sy_spin_unlock(&bl->lock);
        return 0;
/*

        if(list_empty(&bl->lock_list)) {
                bits_lock_clear(bl, pos, 1);
                DINFO("unlock clear, %p, pos: %d\r\n", bl, pos);
                sy_spin_unlock(&bl->lock);
                return 0;
        } else {
                bits_lock_entry_t *entry = (bits_lock_entry_t *)bl->lock_list.next;
                DINFO("unlock resume: %p\r\n", entry->task);
                list_del_init(&entry->list_entry);
                schedule_resume(&entry->task, 0, NULL);

                free(entry);

                sy_spin_unlock(&bl->lock);
                return 0;
        }*/
}

#endif
